振动抑制功能使用说明文档

[TOC]

1. 离线轨迹方案

1.1 接口介绍

/**
 * @brief 计算振动抑制整形后的离线轨迹
 * @param level: 振动抑制等级(1~3), 等级越高抑制效果越明显响应速度相应变慢
 * @param calib_traj: 辨识轨迹, 具体为实际速度、参考位置、速度、加速度
 * @param shape_traj: 整形轨迹, 具体为指令位置、指令速度
 * @return if < 0 计算失败
 */
   ARAL_API_COMMON(1.0) int mcCalVibSuppressShapeTraj(const int& level, const DoubleVecVec& calib_traj, DoubleVecVec& shape_traj) = 0;

适用范围：

• 目前支持关节空间轨迹整形，输出的也是关节空间指令（笛卡尔空间轨迹整形后续支持）
• 对参考速度要求有稳态段，

1.2 使用步骤

S1：真机运行辨识轨迹，采集存储实际速度、参考位置、速度、加速度（规划器给出）数据

S2：调用mcCalVibSuppressShapeTraj接口输入S1存储的数据，计算整形后的轨迹并存储

S3：真机离线运行S2输出的整形轨迹

示例

    TEST_FIXTURE(AuboRobotInterface, testmcCalVibSuppressShapeTraj)
    {
        Setup("aubo_i5");

        S1: //! 数据读取
        interface::DoubleVecVec calib_data, shape_data;
        ReadFromFile2D("./test/data/control/calib_data.csv", ",", calib_data); // cur_vel, ref_pos, ref_vel, ref_acc

        S2: //! 计算输出
        int        N   = calib_data.size();
        const int& dof = robot->mdlGetRobotDOF();
        interface::DoubleVecVec input(N), output(N);
        for (int i = 0; i < N; i++)
        {
            input[i].resize(4 * dof);
            for (int j = 0; j < dof; j++)
            {
                input[i][j]           = calib_data[i][j];
                input[i][j + dof]     = calib_data[i][j + dof];
                input[i][j + dof * 2] = calib_data[i][j + dof * 2];
                input[i][j + dof * 3] = calib_data[i][j + dof * 3];
            }
        }
        robot->mcCalVibSuppressShapeTraj(3, input, output);

        S3: //! 存储整形轨迹
        WriteToFile2D(output, "build/shape_traj.offt");

2. 离线辨识+在线抑制方案

2.1 接口介绍

/**
 * @brief 振动参数辨识
 * @param cur_vel: 实际速度
 * @param ref_vel: 参考速度
 * @param ref_acc: 参考加速度
 * @param omega: 振动频率
 * @param zeta: 振动阻尼比
 * @return if < 0, 辨识失败
 */
int CalibVibrationParams(const interface::DoubleVec& cur_vel, const interface::DoubleVec& ref_vel, const interface::DoubleVec& ref_acc, double& omega, double& zeta);

C函数形式，参数 cur_vel、ref_vel、ref_acc 为单个维度的数据（例如某个关节，n行1列），输出 omega、zeta 则为该维度的振动参数

    /**
     * @brief 设置振动抑制参数
     * @param level: 振动抑制等级(1~3), 等级越高抑制效果越明显响应速度相应变慢
     * @param omega: 振动频率, 长度一般为笛卡尔维度或关节自由度, 单位: Hz
     * @param zeta:  振动阻尼比, 长度一般为笛卡尔维度或关节自由度, 无量纲
     * 频率和阻尼比具体值可配合 CalibVibrationParams 使用
     * @return if < 0 设置失败
     */
    ARAL_API_COMMON(1.0) int mcSetVibSuppressParams(const int& level, const interface::DoubleVec& omega, const interface::DoubleVec& zeta) = 0;

    /**
     * @brief 计算振动抑制整形后的轨迹(周期性调用)
     * @param origin_pos: 整形前位置
     * @param shaped_pos: 整形后位置
     * @return if < 0 计算失败
     */
    ARAL_API_COMMON(1.0) int mcCalVibSuppressShapeTraj(const interface::DoubleVec& origin_pos, interface::DoubleVec& shaped_pos) = 0;

整形前位置为规划器输出的参考位置，开启振动抑制表示调用mcCalVibSuppressShapeTraj 接口将规划器输出轨迹进行在线整形，整形后再将 shaped_pos 下发驱动器执行

2.2 使用步骤

S1（离线辨识）：运行离线辨识轨迹（具体未确定），采集数据：实际速度、参考速度、参考加速度；调用 CalibVibrationParams 接口输入S1中采集的数据，计算出各维度振动参数

示例

    TEST_FIXTURE(AuboRobotInterface, testCalibVibrationParams)
    {
        Setup("aubo_i5");

        //! 读取辨识采集的数据
        interface::DoubleVecVec data;
        ReadFromFile2D("./test/data/control/calib_traj_data.csv", ",", data);
        for (unsigned int j = 0; j < robot->mdlGetRobotDOF(); j++)
        {
            int N = data.size();
            interface::DoubleVec cur_vel(N), ref_vel(N), ref_acc(N);
            for (int i = 0; i < N; i++)
            {
                cur_vel[i] = data[i][j];
                ref_vel[i] = data[i][j + dof];
                ref_acc[i] = data[i][j + dof * 2];
            }

            //! 计算输出
            double omega = 0, zeta = 0;
            ARAL::pack::CalibVibrationParams(cur_vel, ref_vel, ref_acc, omega, zeta);
        }
    }

S2（在线抑制）：调用 mcCalVibSuppressShapeTraj 接口对规划器输出的参考轨迹进行整形，将整形后的轨迹下发驱动器执行

示例

    TEST_FIXTURE(AuboRobotInterface, testRealTimeCalVibSuppressShapeTraj)
    {
        Setup("aubo_i5");
                    interface::DoubleVecVec disable_data;

        ReadFromFile2D("./test/data/control/disable_data.csv", ",", disable_data);   // cur_pos, cur_vel, ref_pos, ref_vel, ref_acc
            int N = disable_data.size();
            interface::DoubleVecVec disable_ref_pos(N);
            for (int i = 0; i < N; i++)
            {
                disable_ref_pos[i].resize(dof);
                for (int j = 0; j < dof; j++)
                {
                    disable_ref_pos[i][j] = disable_data[i][j + dof * 2];
                }
            }

            robot->mcSetControlType(interface::ControlMode::MOTION);
            robot->mcInitiateControlPara(interface::DescribeSpace::FULL, interface::RLPose());

            interface::DoubleVec omega = {4.947, 4.947, 4.947, 4.166, 5.208, 4.947};
            interface::DoubleVec zeta  = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
            robot->mcSetVibSuppressParams(2, omega, zeta);

            int i = 0;
            while (i < N)
            {
                interface::DoubleVec origin_pos(dof);
                memcpy(origin_pos.data(), disable_ref_pos[i].data(), sizeof (double) * dof);  // 未整形的参考位置
                interface::DoubleVec shape_pos(dof);
                robot->mcCalVibSuppressShapeTraj(origin_pos, shape_pos);
                i++;
            }
    }